#include "MOTOR/MPU6050.h"
#include <stdint.h>
#include <math.h>


extern PID_HandleTypeDef angle_pid;      // 角度环 PID 参数

extern float target_angle;        // 目标角度 (单位：度)

extern float current_angle;       // 当前角度

extern float position_x;          // 船体 X 方向位置 (单位：米)
extern float position_y;          // 船体 Y 方向位置 (单位：米)

float current_pitch;
float current_roll;



#if (MPU6050_MODE == 0)

// 写寄存器函数
static void MPU6050_Write_Reg(I2C_HandleTypeDef *hi2c, uint8_t reg, uint8_t data) 
{
    uint8_t buf[2] = {reg, data};
    HAL_I2C_Master_Transmit(hi2c, MPU6050_ADDR, buf, 2, HAL_MAX_DELAY);
}

// 读寄存器函数
static uint8_t MPU6050_Read_Reg(I2C_HandleTypeDef *hi2c, uint8_t reg) 
{
    uint8_t data;
    HAL_I2C_Master_Transmit(hi2c, MPU6050_ADDR, &reg, 1, HAL_MAX_DELAY);
    HAL_I2C_Master_Receive(hi2c, MPU6050_ADDR, &data, 1, HAL_MAX_DELAY);
    return data;
}

// 初始化 MPU6050
void MPU6050_Init(I2C_HandleTypeDef *hi2c)
{
    // 唤醒 MPU6050（清除睡眠模式）
    MPU6050_Write_Reg(hi2c, MPU6050_MPU6050_PWR_MGMT_1, 0x00);
}

// 读取加速度计数据
void MPU6050_Read_Accel(int16_t *ax, int16_t *ay, int16_t *az)
{
    uint8_t buf[6];
    HAL_I2C_Master_Transmit(&hi2c1, MPU6050_ADDR, (uint8_t[]){MPU6050_MPU6050_ACCEL_XOUT_H}, 1, HAL_MAX_DELAY);
    HAL_I2C_Master_Receive(&hi2c1, MPU6050_ADDR, buf, 6, HAL_MAX_DELAY);

    *ax = (int16_t)(buf[0] << 8 | buf[1]);
    *ay = (int16_t)(buf[2] << 8 | buf[3]);
    *az = (int16_t)(buf[4] << 8 | buf[5]);
}

// 读取陀螺仪数据
void MPU6050_Read_Gyro(int16_t *gx, int16_t *gy, int16_t *gz) 
{
    uint8_t buf[6];
    HAL_I2C_Master_Transmit(&hi2c1, MPU6050_ADDR, (uint8_t[]){MPU6050_MPU6050_GYRO_XOUT_H}, 1, HAL_MAX_DELAY);
    HAL_I2C_Master_Receive(&hi2c1, MPU6050_ADDR, buf, 6, HAL_MAX_DELAY);

    *gx = (int16_t)(buf[0] << 8 | buf[1]);
    *gy = (int16_t)(buf[2] << 8 | buf[3]);
    *gz = (int16_t)(buf[4] << 8 | buf[5]);
}

// 校准 MPU6050
void MPU6050_Calibrate(int16_t *ax_offset, int16_t *ay_offset, int16_t *az_offset,int16_t *gx_offset, int16_t *gy_offset, int16_t *gz_offset) 
{
    int32_t ax_sum = 0, ay_sum = 0, az_sum = 0;
    int32_t gx_sum = 0, gy_sum = 0, gz_sum = 0;

    int16_t ax, ay, az, gx, gy, gz;
    for (int i = 0; i < 1000; i++) {
        MPU6050_Read_Accel(&ax, &ay, &az);
        MPU6050_Read_Gyro(&gx, &gy, &gz);

        ax_sum += ax;
        ay_sum += ay;
        az_sum += az;
        gx_sum += gx;
        gy_sum += gy;
        gz_sum += gz;

        HAL_Delay(1);  // 等待一段时间
    }

    *ax_offset = ax_sum / 1000;
    *ay_offset = ay_sum / 1000;
    *az_offset = az_sum / 1000 - 16384;  // 假设 Z 轴重力为 1g
    *gx_offset = gx_sum / 1000;
    *gy_offset = gy_sum / 1000;
    *gz_offset = gz_sum / 1000;
}

#elif (MPU6050_MODE == 1)

// 互补滤波器参数 
static float alpha = 0.98f;  // 陀螺仪权重 
static float dt = 0.01f;     // 采样周期(秒)，根据实际更新频率调整 

// 上一次的陀螺仪数据 
static float last_gx = 0.0f, last_gy = 0.0f, last_gz = 0.0f;
 
// 低通滤波后的加速度计数据 
static float filtered_ax = 0.0f, filtered_ay = 0.0f, filtered_az = 0.0f;


// 初始化 MPU6050
void MPU6050_Init(I2C_HandleTypeDef *hi2c) 
{
    uint8_t data = 0x00;  // 唤醒 MPU6050
    HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, MPU6050_PWR_MGMT_1, 1, &data, 1, HAL_MAX_DELAY);
    //初始化陀螺仪
    MPU_Data.pitch = 0.0f;
    MPU_Data.roll = 0.0f;
    MPU_Data.yaw = 0.0f;
}

// 读取 MPU6050 数据
void MPU6050_Read_Raw(I2C_HandleTypeDef *hi2c,int16_t *accel_data, int16_t *gyro_data) 
{
    uint8_t buffer[14];
    HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR, MPU6050_ACCEL_XOUT_H, 1, buffer, 14, HAL_MAX_DELAY);

    accel_data[0] = (int16_t)((buffer[0] << 8) | buffer[1]);  // Accel X
    accel_data[1] = (int16_t)((buffer[2] << 8) | buffer[3]);  // Accel Y
    accel_data[2] = (int16_t)((buffer[4] << 8) | buffer[5]);  // Accel Z

    gyro_data[0] = (int16_t)((buffer[8] << 8) | buffer[9]);   // Gyro X
    gyro_data[1] = (int16_t)((buffer[10] << 8) | buffer[11]); // Gyro Y
    gyro_data[2] = (int16_t)((buffer[12] << 8) | buffer[13]); // Gyro Z
}

// 计算姿态角 (互补滤波)
void Compute_Angles(int16_t *accel_data, int16_t *gyro_data, float dt) 
{
    static float pitch_acc, roll_acc;

    // 低通滤波加速度计数据 (减少高频噪声)
    float beta = 0.1f; // 低通滤波系数 
    filtered_ax = beta * accel_data[0] + (1 - beta) * filtered_ax;
    filtered_ay = beta * accel_data[1] + (1 - beta) * filtered_ay;
    filtered_az = beta * accel_data[2] + (1 - beta) * filtered_az;

    // 加速度计计算角度
    float accel_x = filtered_ax / 16384.0f;  // 转换为 g
    float accel_y = filtered_ay / 16384.0f;
    float accel_z = filtered_az / 16384.0f;

    pitch_acc = atan2(-accel_x, sqrt(accel_y * accel_y + accel_z * accel_z)) * 180.0f / M_PI;
    roll_acc = atan2(accel_y, accel_z) * 180.0f / M_PI;

    // // 陀螺仪计算角度变化
    // float gyro_x = gyro_data[0] / 131.0f;  // 转换为 deg/s
    // float gyro_y = gyro_data[1] / 131.0f;

    // pitch_gyro += gyro_x * dt;
    // roll_gyro += gyro_y * dt;

    // // 互补滤波
    // current_pitch = 0.98f * (current_pitch + gyro_x * dt) + 0.02f * pitch_acc;
    // current_roll = 0.98f * (current_roll + gyro_y * dt) + 0.02f * roll_acc;

    // 陀螺仪计算姿态角变化 (互补滤波)
    float gyro_roll = MPU_Data.roll  + (gyro_data[0] + last_gx) * 0.5f * dt;
    float gyro_pitch = MPU_Data.pitch  + (gyro_data[1] + last_gy) * 0.5f * dt;
    float gyro_yaw = MPU_Data.yaw  + (gyro_data[2] + last_gz) * 0.5f * dt;
    
    // 保存当前陀螺仪数据用于下一次计算 
    last_gx = gyro_data[0];
    last_gy = gyro_data[1];
    last_gz = gyro_data[2];
    
    // 互补滤波融合加速度计和陀螺仪数据 
    MPU_Data.roll   = alpha * gyro_roll + (1 - alpha) * roll_acc;
    MPU_Data.pitch  = alpha * gyro_pitch + (1 - alpha) * pitch_acc;
    MPU_Data.yaw    = gyro_yaw;  // 加速度计无法测量偏航角，完全依赖陀螺仪 
    
    // 转换为角度 
    MPU_Data.roll   *= 180.0f / M_PI;  // 180/PI 
    MPU_Data.pitch  *= 180.0f / M_PI;
    MPU_Data.yaw    *= 180.0f / M_PI;
}

// // 控制函数
// void Ship_Control(TIM_HandleTypeDef *htim_pwm, I2C_HandleTypeDef *hi2c) 
//{
//     static uint32_t last_time = 0;
//     uint32_t current_time = HAL_GetTick();
//     float dt = (float)(current_time - last_time) / 1000.0f;

//     if (dt > 0) {
//         int16_t accel_data[3], gyro_data[3];

//         // 读取 MPU6050 数据
//         MPU6050_Read_Raw(accel_data, gyro_data);

//         // 计算姿态角
//         Compute_Angles(accel_data, gyro_data, dt);

//         // 控制船体姿态
//         Ship_Attitude_Control(htim_pwm);

//         // 计算船体位置
//         Get_Ship_Position(accel_data, dt);

//         last_time = current_time;
//     }
// }

//测试函数
void MPU6050_SendData(I2C_HandleTypeDef *hi2c,UART_HandleTypeDef *huart,int16_t *accel_data, int16_t *gyro_data)
{
    // 读取 MPU6050 数据
    MPU6050_Read_Raw(hi2c,accel_data, gyro_data);

    // 计算姿态角
    Compute_Angles(accel_data, gyro_data, dt);

    //发送MPU6050数据
    USART_printf(huart,"%d,%d,%d,%d,%d,%d\r\n",accel_data[0],accel_data[1],accel_data[2],gyro_data[0],gyro_data[1],gyro_data[2]);

    USART_printf(huart,"%.2f,%.2f,%.2f\r\n",MPU_Data.pitch,MPU_Data.roll,MPU_Data.yaw);
}

#elif MPU6050_MODE == 2

//使用加速度计算《加速度欧拉角》,_a的意思就是用的加速度
float roll_a, pitch_a;
//使用角速度计算《角速度欧拉角》,_g的意思就是用的角速度
float roll_g, pitch_g, yaw_g;

const float alpha = 0.9052;//这个0.9可以改，0-1之间的数，一般在0.95往上，一次增加0.01调试一下
const float yawi = 0.9;

void MPU6050_Init(I2C_HandleTypeDef *hi2c)
{
	uint8_t check;
	uint8_t Data;

	HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR, WHO_AM_I_REG, 1, &check, 1, 1000);

	if (check == 0x68)//寄存器文档最后一页写了WHO_AM_I_REG就是0x68
	{
		//电源管理1
		Data = 0x00;
		HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &Data, 1, 1000);
		//电源管理2
		Data = 0x00;
		HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, PWR_MGMT_2_REG, 1, &Data, 1, 1000);
		//滤波
		Data = 0x06;
		HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, MPU6050_CONFIG, 1, &Data, 1, 1000);
		//采样频率分频器寄存器
		Data = 0x09;
		HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, SMPLRT_DIV_REG, 1, &Data, 1, 1000);
		//加速度计配置
		Data = 0x01;
		HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, ACCEL_CONFIG_REG, 1, &Data, 1, 1000);
		//陀螺仪配置
		Data = 0x18;
		HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, GYRO_CONFIG_REG, 1, &Data, 1, 1000);
	}
}

void MPU6050_Read_Accel(I2C_HandleTypeDef *hi2c)//读取加速度
{
	uint8_t Rec_Data[6];
	HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 6, 1000);
	MPU_Data.accel_data[0] = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
	MPU_Data.accel_data[1] = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
	MPU_Data.accel_data[2] = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
	MPU_Data.accel_data[0] = MPU_Data.accel_data[0] * 6.1035e-5f; // 2048.0;
	MPU_Data.accel_data[1] = MPU_Data.accel_data[1] * 6.1035e-5f; // 2048.0;
	MPU_Data.accel_data[2] = MPU_Data.accel_data[2] * 6.1035e-5f; // 2048.0;//此处多减0.5是我的硬件问题，如果你硬件是好的，就不用减
}

void MPU6050_Read_Gyro(I2C_HandleTypeDef *hi2c)//读取角速度
{
	uint8_t Rec_Data[6];
	HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data, 6, 1000);
	MPU_Data.gyro_data[0] = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
	MPU_Data.gyro_data[1] = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
	MPU_Data.gyro_data[2] = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
	MPU_Data.gyro_data[0] = MPU_Data.gyro_data[0] * 6.1035e-2f; // 16.4;
	MPU_Data.gyro_data[1] = MPU_Data.gyro_data[1] * 6.1035e-2f; // 16.4;
	MPU_Data.gyro_data[2] = MPU_Data.gyro_data[2] * 6.1035e-2f; // 16.4;
}


//此函数是读取结果，当然这个结果是互补滤波法计算的
//如果你想读取yaw roll pitch 仅需调用此函数
void MPU6050_Read_Result(I2C_HandleTypeDef *hi2c)
{

	//上面函数MPU6050_Read_Accel()中的，改了下变量名，让大家更易懂，你也可以直接调用函数
	uint8_t Rec_Data_A[6];
	HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data_A, 6, 1000);
	MPU_Data.accel_data[0] = (int16_t)(Rec_Data_A[0] << 8 | Rec_Data_A[1]);
	MPU_Data.accel_data[1] = (int16_t)(Rec_Data_A[2] << 8 | Rec_Data_A[3]);
	MPU_Data.accel_data[2] = (int16_t)(Rec_Data_A[4] << 8 | Rec_Data_A[5]);
	MPU_Data.accel_data[0] = MPU_Data.accel_data[0] * 6.1035e-5f;
	MPU_Data.accel_data[1] = MPU_Data.accel_data[1] * 6.1035e-5f;
	MPU_Data.accel_data[2] = MPU_Data.accel_data[2] * 6.1035e-5f;

	//上面函数MPU6050_Read_Gyro()中的，改了下变量名，让大家更易懂，你也可以直接调用函数
	uint8_t Rec_Data_G[6];
	HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data_G, 6, 1000);
	MPU_Data.gyro_data[0] = (int16_t)(Rec_Data_G[0] << 8 | Rec_Data_G[1]);
	MPU_Data.gyro_data[1] = (int16_t)(Rec_Data_G[2] << 8 | Rec_Data_G[3]);
	MPU_Data.gyro_data[2] = (int16_t)(Rec_Data_G[4] << 8 | Rec_Data_G[5]);
	MPU_Data.gyro_data[0] = MPU_Data.gyro_data[0] * 6.1035e-2f;
	MPU_Data.gyro_data[1] = MPU_Data.gyro_data[1] * 6.1035e-2f;
	MPU_Data.gyro_data[2] = MPU_Data.gyro_data[2] * 6.1035e-2f;


	//使用加速度计算欧拉角，如果用3.1415926，计算结果会慢，C8T6算小数太慢
	pitch_a = atan2(MPU_Data.accel_data[1], MPU_Data.accel_data[2]) * 180.0f / M_PI;
	roll_a = - atan2(MPU_Data.accel_data[0], MPU_Data.accel_data[2]) * 180.0f / M_PI;

	//使用角速度计算欧拉角
    pitch_g = MPU_Data.pitch + MPU_Data.gyro_data[0] * 0.005;
	roll_g = MPU_Data.roll - MPU_Data.gyro_data[1] * 0.005;
	yaw_g = MPU_Data.yaw + MPU_Data.gyro_data[2] * 0.005;

	//进行互补融合
	// MPU_Data.pitch = MPU_Data.pitch + (pitch_a - pitch_g) *alpha;    
	// MPU_Data.roll = MPU_Data.roll + (roll_a - roll_g ) *alpha;
    MPU_Data.pitch = alpha * pitch_g + (1 - alpha) * pitch_a;    
	MPU_Data.roll = alpha * roll_g + (1 - alpha) * roll_a;
	MPU_Data.yaw = yaw_g * yawi + MPU_Data.yaw * (1 - yawi);

}


//测试函数
void MPU6050_SendData(I2C_HandleTypeDef *hi2c,UART_HandleTypeDef *huart)
{
    // 计算姿态角
    MPU6050_Read_Result(hi2c);

    //发送MPU6050数据
    USART_printf(huart,"%d,%d,%d,%d,%d,%d,%.2f,%.2f,%.2f\r\n",MPU_Data.accel_data[0],MPU_Data.accel_data[1],MPU_Data.accel_data[2],
                                                        MPU_Data.gyro_data[0],MPU_Data.gyro_data[1],MPU_Data.gyro_data[2]
                                                        ,MPU_Data.pitch,MPU_Data.roll,MPU_Data.yaw);

}


#elif MPU6050_MODE == 3

 
#define delta_T 0.002f // 2ms计算一次      这里的时间可以改成5ms

float Q_info_q0 = 1, Q_info_q1 = 0, Q_info_q2 = 0, Q_info_q3 = 0;

float I_ex, I_ey, I_ez; // 误差积分

float param_Kp = 0.17;  // 加速度计的收敛速率比例增益
float param_Ki = 0.004; // 陀螺仪收敛速率的积分增益 0.004
/**  
  * 函数：MPU6050写寄存器  
  * 参数：RegAddress 寄存器地址  
  * 参数：Data 要写入寄存器的数据  
  * 返回值：无  
  */  
void MPU6050_WriteReg(I2C_HandleTypeDef *hi2c,uint8_t RegAddress, uint8_t Data)  
{  
    HAL_StatusTypeDef status;  
  
    // 发送设备地址 + 写操作 + 寄存器地址  
    status = HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR << 1, RegAddress, I2C_MEMADD_SIZE_8BIT, &Data, 1, HAL_MAX_DELAY);  
    if (status != HAL_OK)  
    {  
        // 错误处理  
        // 可以添加代码来处理错误，比如重试或记录错误  
    }  
}  
  
/**  
  * 函数：MPU6050读寄存器  
  * 参数：RegAddress 寄存器地址  
  * 返回值：读取寄存器的数据  
  */  
uint8_t MPU6050_ReadReg(I2C_HandleTypeDef *hi2c,uint8_t RegAddress)  
{  
    uint8_t Data;  
    HAL_StatusTypeDef status;  
  
    // 发送设备地址 + 写操作 + 寄存器地址（为了告诉MPU6050我们要读哪个寄存器）  
    status = HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR << 1, RegAddress, I2C_MEMADD_SIZE_8BIT, &RegAddress, 1, HAL_MAX_DELAY);  
    if (status != HAL_OK)  
    {  
        // 错误处理  
        return 0xFF; // 或者其他错误码  
    }  
  
    // 发送设备地址 + 读操作（从之前指定的寄存器地址读取数据）  
    status = HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR << 1, RegAddress, I2C_MEMADD_SIZE_8BIT, &Data, 1, HAL_MAX_DELAY);  
    if (status != HAL_OK)  
    {  
        // 错误处理  
        return 0xFF; // 或者其他错误码  
    }  
  
    return Data;  
}  


// /**
//   * 函    数：MPU6050初始化
//   * 参    数：无
//   * 返 回 值：无
//   */
// void MPU6050_Init(I2C_HandleTypeDef *hi2c)
// {
	
// 	/*MPU6050寄存器初始化，需要对照MPU6050手册的寄存器描述配置，此处仅配置了部分重要的寄存器*/
// 	MPU6050_WriteReg(hi2c,MPU6050_PWR_MGMT_1, 0x01);		//电源管理寄存器1，取消睡眠模式，选择时钟源为X轴陀螺仪
// 	MPU6050_WriteReg(hi2c,MPU6050_PWR_MGMT_2, 0x00);		//电源管理寄存器2，保持默认值0，所有轴均不待机
// 	MPU6050_WriteReg(hi2c,MPU6050_SMPLRT_DIV, 0x09);		//采样率分频寄存器，配置采样率
// 	MPU6050_WriteReg(hi2c,MPU6050_CONFIG, 0x06);			//配置寄存器，配置DLPF
// 	MPU6050_WriteReg(hi2c,MPU6050_GYRO_CONFIG, 0x18);	//陀螺仪配置寄存器，选择满量程为±2000°/s
// 	MPU6050_WriteReg(hi2c,MPU6050_ACCEL_CONFIG, 0x18);	//加速度计配置寄存器，选择满量程为±16g
// }
 
/**
  * 函    数：MPU6050初始化
  * 参    数：无
  * 返 回 值：无
  */
void MPU6050_Init(I2C_HandleTypeDef *hi2c)
{
	uint8_t check;
	uint8_t Data;

	HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR, WHO_AM_I_REG, 1, &check, 1, 1000);

	if (check == 0x68)//寄存器文档最后一页写了WHO_AM_I_REG就是0x68
	{
		//电源管理1
		Data = 0x01;
		HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &Data, 1, 1000);
		//电源管理2
		Data = 0x00;
		HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, PWR_MGMT_2_REG, 1, &Data, 1, 1000);
		//滤波
		Data = 0x06;
		HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, MPU6050_CONFIG, 1, &Data, 1, 1000);
		//采样频率分频器寄存器
		Data = 0x09;
		HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, SMPLRT_DIV_REG, 1, &Data, 1, 1000);
		//加速度计配置
		Data = 0x18;
		HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, ACCEL_CONFIG_REG, 1, &Data, 1, 1000);
		//陀螺仪配置
		Data = 0x18;
		HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, GYRO_CONFIG_REG, 1, &Data, 1, 1000);
	}
}

/**
  * 函    数：MPU6050获取ID号
  * 参    数：无
  * 返 回 值：MPU6050的ID号
  */
uint8_t MPU6050_GetID(I2C_HandleTypeDef *hi2c)
{
	return MPU6050_ReadReg(hi2c,MPU6050_WHO_AM_I);		//返回WHO_AM_I寄存器的值
}
 
/**
  * 函    数：MPU6050获取数据
  * 参    数：AccX AccY AccZ 加速度计X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
  * 返 回 值：无
  */
void MPU6050_GetData_ACC(I2C_HandleTypeDef *hi2c)
{
	uint8_t DataH, DataL;								//定义数据高8位和低8位的变量
	
	DataH = MPU6050_ReadReg(hi2c,MPU6050_ACCEL_XOUT_H);		//读取加速度计X轴的高8位数据
	DataL = MPU6050_ReadReg(hi2c,MPU6050_ACCEL_XOUT_L);		//读取加速度计X轴的低8位数据
	MPU_Data.accel_data[0] = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
	
	DataH = MPU6050_ReadReg(hi2c,MPU6050_ACCEL_YOUT_H);		//读取加速度计Y轴的高8位数据
	DataL = MPU6050_ReadReg(hi2c,MPU6050_ACCEL_YOUT_L);		//读取加速度计Y轴的低8位数据
	MPU_Data.accel_data[1] = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
	
	DataH = MPU6050_ReadReg(hi2c,MPU6050_ACCEL_ZOUT_H);		//读取加速度计Z轴的高8位数据
	DataL = MPU6050_ReadReg(hi2c,MPU6050_ACCEL_ZOUT_L);		//读取加速度计Z轴的低8位数据
	MPU_Data.accel_data[2] = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
}
 
/**
  * 函    数：MPU6050获取数据角速度
  * 参    数：GyroX GyroY GyroZ 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
  * 返 回 值：无
  */
void MPU6050_GetData_GYRO(I2C_HandleTypeDef *hi2c)
{
    uint8_t DataH, DataL;								//定义数据高8位和低8位的变量
 
    DataH = MPU6050_ReadReg(hi2c,MPU6050_GYRO_XOUT_H);		//读取陀螺仪X轴的高8位数据
	DataL = MPU6050_ReadReg(hi2c,MPU6050_GYRO_XOUT_L);		//读取陀螺仪X轴的低8位数据
	MPU_Data.gyro_data[0] = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
	
	DataH = MPU6050_ReadReg(hi2c,MPU6050_GYRO_YOUT_H);		//读取陀螺仪Y轴的高8位数据
	DataL = MPU6050_ReadReg(hi2c,MPU6050_GYRO_YOUT_L);		//读取陀螺仪Y轴的低8位数据
	MPU_Data.gyro_data[1] = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
	
	DataH = MPU6050_ReadReg(hi2c,MPU6050_GYRO_ZOUT_H);		//读取陀螺仪Z轴的高8位数据
	DataL = MPU6050_ReadReg(hi2c,MPU6050_GYRO_ZOUT_L);		//读取陀螺仪Z轴的低8位数据
	MPU_Data.gyro_data[2] = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
}
 
/**
  * 函    数：MPU6050实际数据角     转化为带物理单位的数据，单位为m/(s^2)
  * 参    数：T_GyroX T_GyroY T_GyroZ 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回
  * 返 回 值：无
  */
void MPU6050_transition_GYRO(I2C_HandleTypeDef *hi2c,int16_t *T_GyroX, int16_t *T_GyroY, int16_t *T_GyroZ)
{
    MPU6050_GetData_GYRO(hi2c);
    *T_GyroX=MPU_Data.gyro_data[0]/32768*16*9.8;
    *T_GyroY=MPU_Data.gyro_data[1]/32768*16*9.8;
    *T_GyroY=MPU_Data.gyro_data[2]/32768*16*9.8;   
}
 
/**
  * 函    数：MPU6050实际加速度     转化为带物理单位的数据，单位：g(m/s^2)
  * 参    数：T_AccX T_AccY T_AccZ 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回
  * 返 回 值：无
  */
void MPU6050_transition_ACC(I2C_HandleTypeDef *hi2c,int16_t *T_AccX, int16_t *T_AccY, int16_t *T_AccZ)
{
    MPU6050_GetData_ACC(hi2c);
    *T_AccX=MPU_Data.accel_data[0]/16384;
    *T_AccY=MPU_Data.accel_data[1]/16384;
    *T_AccY=MPU_Data.accel_data[2]/16384;   
}  


// 互补滤波
void ICM_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az) 
{
    float halfT = 0.5f * delta_T;
    float vx, vy, vz; // 当前的机体坐标系上的重力单位向量
    float ex, ey, ez; // 四元数计算值与加速度计测量值的误差
    float q0 = Q_info_q0;
    float q1 = Q_info_q1;
    float q2 = Q_info_q2;
    float q3 = Q_info_q3;
    float q0q0 = q0 * q0;
    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    //float q0q3 = q0 * q3;
    float q1q1 = q1 * q1;
    //float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q2 = q2 * q2;
    float q2q3 = q2 * q3;
    float q3q3 = q3 * q3;
    // float delta_2 = 0;
 
    // 对加速度数据进行归一化 得到单位加速度
    float norm = sqrtf(ax * ax + ay * ay + az * az);
 
    ax = ax * norm;
    ay = ay * norm;
    az = az * norm;
 
    // 根据当前四元数的姿态值来估算出各重力分量。用于和加速计实际测量出来的各重力分量进行对比，从而实现对四轴姿态的修正
    vx = 2 * (q1q3 - q0q2);
    vy = 2 * (q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;
    // vz = (q0*q0-0.5f+q3 * q3) * 2;
 
    // 叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。
    ex = ay * vz - az * vy;
    ey = az * vx - ax * vz;
    ez = ax * vy - ay * vx;
 
    // 用叉乘误差来做PI修正陀螺零偏，
    // 通过调节 param_Kp，param_Ki 两个参数，
    // 可以控制加速度计修正陀螺仪积分姿态的速度。
    I_ex += halfT * ex; // integral error scaled by Ki
    I_ey += halfT * ey;
    I_ez += halfT * ez;
 
    gx = gx + param_Kp * ex + param_Ki * I_ex;
    gy = gy + param_Kp * ey + param_Ki * I_ey;
    gz = gz + param_Kp * ez + param_Ki * I_ez;
 
    /*数据修正完成，下面是四元数微分方程*/
 
    // 四元数微分方程，其中halfT为测量周期的1/2，gx gy gz为陀螺仪角速度，以下都是已知量，这里使用了一阶龙哥库塔求解四元数微分方程
    q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
    q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
    q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
    q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;
    //    delta_2=(2*halfT*gx)*(2*halfT*gx)+(2*halfT*gy)*(2*halfT*gy)+(2*halfT*gz)*(2*halfT*gz);
    //    整合四元数率    四元数微分方程  四元数更新算法，二阶毕卡法
    //    q0 = (1-delta_2/8)*q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    //    q1 = (1-delta_2/8)*q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    //    q2 = (1-delta_2/8)*q2 + (q0*gy - q1*gz + q3*gx)*halfT;
    //    q3 = (1-delta_2/8)*q3 + (q0*gz + q1*gy - q2*gx)*halfT
 
    // normalise quaternion
    norm = sqrtf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    Q_info_q0 = q0 * norm;
    Q_info_q1 = q1 * norm;
    Q_info_q2 = q2 * norm;
    Q_info_q3 = q3 * norm;
 
}
 
// 获取车辆姿态
void ICM_getEulerianAngles(I2C_HandleTypeDef *hi2c) 
{
    //采集陀螺仪数据
    int16_t gx,gy,gz,ax,ay,az;
    float T_gx,T_gy,T_gz,T_ax,T_ay,T_az;
    MPU6050_transition_GYRO(&hi2c_mpu,&gx,&gy,&gz);
    MPU6050_transition_ACC(&hi2c_mpu,&ax,&ay,&az);

    
    //数据类型转化
    T_gx = gx * 1.0f;
    T_gy = gy * 1.0f;
    T_gz = gz * 1.0f;
    T_ax = ax * 1.0f;
    T_ay = ay * 1.0f;
    T_az = az * 1.0f;


    // 互补滤波
    ICM_AHRSupdate(T_gx,T_gy,T_gz,T_ax,T_ay,T_az);


    //四元数计算欧拉角---滤波数据
    float q0 = Q_info_q0;
    float q1 = Q_info_q1;
    float q2 = Q_info_q2;
    float q3 = Q_info_q3;
    MPU_Data.roll = -asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / M_PI;                                // pitch
    MPU_Data.pitch = -atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / M_PI; // roll
    MPU_Data.yaw_t = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / M_PI;  // yaw
    float i = MPU_Data.yaw_t - MPU_Data.yaw_last;
    if (i < -180) {
        i += 360;
    }
    else if (i > 180) {
        i -= 360;
    }
    MPU_Data.yaw += i;
    MPU_Data.yaw_last = MPU_Data.yaw;


    //姿态限制


    //数据测试
    USART_printf(Test_huart,"%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",MPU_Data.accel_data[0],MPU_Data.accel_data[1],MPU_Data.accel_data[2]
                                        ,MPU_Data.gyro_data[0],MPU_Data.gyro_data[1],MPU_Data.gyro_data[2],MPU_Data.pitch,MPU_Data.roll,MPU_Data.yaw);

}

// // 获取车辆姿态
// void ICM_getEulerianAngles(void) 
// {
//     // 采集陀螺仪数据
//     MPU_Get_Gyroscope();
//     MPU_Get_Accelerometer();
//     mpu6050_acc_x = mpu6050_acc_transition(mpu6050_acc_x);
//     mpu6050_acc_y = mpu6050_acc_transition(mpu6050_acc_y);
//     mpu6050_acc_z = mpu6050_acc_transition(mpu6050_acc_z);
//     ICM_getValues();
//     ICM_AHRSupdate(icm_data_gyro_x, icm_data_gyro_y, icm_data_gyro_z, icm_data_acc_x, icm_data_acc_y, icm_data_acc_z);
//     // ICM_AHRSupdate(icm_data_gyro_y, icm_data_gyro_z, icm_data_gyro_x, icm_data_acc_y, icm_data_acc_z, icm_data_acc_x);//修改陀螺仪位置后，改这里，姿态解算就不会出错啦
 
//      // ZX XY  YZ
//      // xy  yz zx
//     float q0 = Q_info_q0;
//     float q1 = Q_info_q1;
//     float q2 = Q_info_q2;
//     float q3 = Q_info_q3;
//     // 四元数计算欧拉角---原始
//     eulerAngle_roll = -asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / M_PI;                                // pitch
//     eulerAngle_pitch = -atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / M_PI; // roll
//     eulerAngle_yaw = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / M_PI;  // yaw
//     float i = eulerAngle_yaw - eulerAngle_yaw_old;
//     if (i < -180) {
//         i += 360;
//     }
//     else if (i > 180) {
//         i -= 360;
//     }
//     eulerAngle_yaw_total += i;
//     eulerAngle_yaw_old = eulerAngle_yaw;
 

//     //姿态限制
 
//     // if (eulerAngle_yaw > 360) {
//     //     eulerAngle_yaw -= 360;
//     // }
//     // else if (eulerAngle_yaw > 0) {
//     //     eulerAngle_yaw += 360;
//     // }
 
// }

#endif

